#include "cvxMatch.h"

CvxMatch::CvxMatch(int threshold, int matchNumber)
{	
	assert(threshold > 0);
	assert(matchNumber > 50);
	m_pExtractor = new BriefDescriptorExtractor(16);
	m_pFast = new FastFeatureDetector(threshold, true);
	m_pDetector = new GridAdaptedFeatureDetector(m_pFast, matchNumber*4, 4, 4);	
	m_pMatcher = new BFMatcher(NORM_HAMMING);

	assert(m_pExtractor);
	assert(m_pFast);
	assert(m_pDetector);
	assert(m_pMatcher);

	m_threshold = threshold;
	m_desiredNumber = matchNumber;

}
CvxMatch::~CvxMatch()
{
	if (m_pFast)
	{
		delete m_pFast, m_pFast = NULL;
	}
	if (m_pExtractor)
	{
		delete m_pExtractor, m_pExtractor = NULL;
	}
	if (m_pDetector)
	{
		delete m_pDetector, m_pDetector = NULL;
	}
	if (m_pMatcher)
	{
		delete m_pMatcher, m_pMatcher = NULL;
	}

}

void CvxMatch::configure(int threshold, int matchNumber)
{
	if (threshold != m_threshold)
	{
		delete m_pFast, m_pFast = NULL;
		m_pFast = new FastFeatureDetector(threshold, true);
		assert(m_pFast);			
	}

	if (m_pDetector)
	{
		delete m_pDetector, m_pDetector = NULL;
	}

	m_pDetector = new GridAdaptedFeatureDetector(m_pFast, matchNumber*4, 4, 4);

	m_threshold = threshold;
	m_desiredNumber = matchNumber;

}
void CvxMatch::match(const Mat &imageA, const Mat &imageB, vector<Point2f> &ptA, vector<Point2f> &ptB)
{
	if( imageA.empty() || imageB.empty())
		return;
	CV_Assert(imageA.size() == imageB.size());
	ptA.clear();
	ptB.clear();

	assert(m_pExtractor);
	assert(m_pFast);
	assert(m_pDetector);
	assert(m_pMatcher);

	vector<KeyPoint> keypoints1, keypoints2;
	m_pDetector->detect(imageA, keypoints1);
	m_pDetector->detect(imageB, keypoints2);

	printf("key point number is %d %d\n", keypoints1.size(), keypoints2.size());


	// computing descriptors
	Mat descriptors1, descriptors2;
	m_pExtractor->compute(imageA, keypoints1, descriptors1);
	m_pExtractor->compute(imageB, keypoints2, descriptors2);

	// matching descriptors
	vector<DMatch> matches;

	m_pMatcher->match(descriptors1, descriptors2, matches);

	vector<Point2f> pts1;
	vector<Point2f> pts2;
	for (int i = 0; i<matches.size(); ++i)
	{
		int i1 = matches[i].queryIdx;
		int i2 = matches[i].trainIdx;
		pts1.push_back(keypoints1[i1].pt);
		pts2.push_back(keypoints2[i2].pt);
	}

	Mat inlierMat;
	Mat F = cv::findFundamentalMat(pts1, pts2, FM_RANSAC, 1, 0.99, inlierMat);

	vector<DMatch> matchInlier;
	for (int i = 0; i<pts1.size(); ++i)
	{
		if (inlierMat.at<unsigned char>(i, 0) != 0)
		{
			matchInlier.push_back(matches[i]);
			int i1 = matches[i].queryIdx;
			int i2 = matches[i].trainIdx;
			ptA.push_back(keypoints1[i1].pt);
			ptB.push_back(keypoints2[i2].pt);
		}
	}
	
//	printf("match numbers = %d -> %d\n", pts1.size(), matchInlier.size());

	// drawing the results
// 	namedWindow("matches", 1);
// 	Mat img_matches;
// 	drawMatches(imageA, keypoints1, imageB, keypoints2, matchInlier, img_matches);
// 	imshow("matches", img_matches);
// 	imwrite("matches.png", img_matches);


}